#include<stdio.h>
#include "gs_algorithm.h"
#include <string.h>
#define LOG_TAG TAG_SNSR
#include <stdlib.h>
#include "math.h"
#include "adapt.h"
#include "elog.h"
#include "qlcommon.h"

void kf_pitch_init(ST_KALMAN_FILTER_PARA * KF_pitch)
{
    KF_pitch->dt = 0.009; /*根据log，83hz，0.001,0.008,0.02(收敛快)*/
    KF_pitch->Q_angle = 0.001;//0.001
    KF_pitch->Q_bias = 1.409976;
    KF_pitch->Q_gyro = 0.025;
    KF_pitch->R_angle = 0.000001332;/*测试过的数据有：0.3;gyro 3segma:0.000001332*/
    KF_pitch->K_0 = 1.0f;   
    KF_pitch->K_1 = 1.0f;
    KF_pitch->angle = 0.0f;
    KF_pitch->PP[0][0] = 1.0f;
    KF_pitch->PP[0][1] = 0.0f;
    KF_pitch->PP[1][0] = 0.0f;
    KF_pitch->PP[1][1] = 1.0f;
}

void kf_roll_init(ST_KALMAN_FILTER_PARA * KF_roll)
{
    KF_roll->dt = 0.009;
    KF_roll->Q_angle = 0.00000134775;
    KF_roll->Q_bias = -0.936;
    KF_roll->Q_gyro = 0.0025;
    KF_roll->R_angle = 0.00000134775;/*用过0.3*/
    KF_roll->K_0 = 1.0f;
    KF_roll->K_1 = 1.0f;
    KF_roll->angle = 0.0f;
    KF_roll->PP[0][0] = 1.0f;
    KF_roll->PP[0][1] = 0.0f;
    KF_roll->PP[1][0] = 0.0f;
    KF_roll->PP[1][1] = 1.0f;
}


/************************************************通用数据处理接口**********************************************************/

/*高通滤波*/
float highPassFilter(float thresh_high_pass, float gyro)
{
    float temp_gyro = 0;

    if((gyro - thresh_high_pass) > 0)
    {
        temp_gyro = gyro;
    }
    if((gyro + thresh_high_pass) < 0)
    {
        temp_gyro =gyro;
    }

    return temp_gyro;
}

/*低通滤波 减少动力加速度对姿态解算的影响*/
/*滤波器系数，取值范围在 0 到 1 之间，越接近1则保留越多低频成分，越接近0则增强高频滤波效果。
较大的 `alpha` 值会保留更多的低频成分，但可能对快速变化的信号响应较慢；较小的 `alpha` 值会增强高频滤波效果，
但可能会丢失一些低频信息*/
float lowPassFilter(float input, float previousOutput) 
{
    float alpha = 0.05f;
    float val = 1.00f;
    
    float output = alpha * input + (val - alpha) * previousOutput;
    
    return output;
}

/*判断异常加速度数据*/
bool judge_measure_angle(float acc_xyz)
{
    /*大于0，表示加速度计的后验估计值不可用*/
    float acc_measure_judege[6] = {0};
    acc_measure_judege[0] = fabsf(acc_xyz - 1.065985) - 0.02;
    acc_measure_judege[1] = fabsf(acc_xyz - 0.95039) - 0.05;
    acc_measure_judege[2] = fabsf(acc_xyz - 1.037251) - 0.02;
    acc_measure_judege[3] = fabsf(acc_xyz - 0.8562) - 0.05;
    acc_measure_judege[4] = fabsf(acc_xyz) - 0.6;
    acc_measure_judege[5] = fabsf(acc_xyz -0.9898) - 0.02;
    if((acc_measure_judege[0] < 0.00001) || (acc_measure_judege[1] < 0.00001) || (acc_measure_judege[2] < 0.00001) || (acc_measure_judege[3] < 0.00001) || (acc_measure_judege[4] < 0.00001)|| (acc_measure_judege[5] < 0.00001) )
    {
        return true;
    }
    else    
    {
        return false;
    }
}

/*卡尔曼滤波*/
ST_KALMAN_FILTER_PARA kalman_filter(float angle_m, float v, ST_KALMAN_FILTER_PARA kfp, ST_ACC st_acc)
{
     /*判断是否在颠簸地面*/
    bool judge = judge_measure_angle(st_acc.acc_xyz);
    if(!judge)
    {
        return kfp;
    }

    /*如果没有角速度，便不进行先验估计了*/
    if(0 == v)
    {
        /*不做处理*/
    }
    else
    {
        /*1.先验估计的roll*/
        kfp.angle += (v - kfp.Q_bias) * kfp.dt;
        /*2.更新系统误差矩阵*/
        kfp.PP[0][0] += (kfp.Q_angle - (kfp.PP[0][1] - kfp.PP[1][0])) * kfp.dt;
        kfp.PP[0][1] += -kfp.PP[1][1] * kfp.dt;
        kfp.PP[1][0] += -kfp.PP[1][1] * kfp.dt;
        kfp.PP[1][1] += kfp.Q_gyro;
        /*3.更新卡尔曼增益*/
        kfp.K_0 = kfp.PP[0][0] / kfp.PP[0][0] + kfp.R_angle;
        kfp.K_1 = kfp.PP[1][0] / kfp.PP[1][0] + kfp.R_angle;
    }

    /*4.后验估计*/
    kfp.angle += kfp.K_0 * (angle_m - kfp.angle);
    kfp.Q_bias += kfp.K_1 * (angle_m - kfp.angle);
    kfp.PP[0][0] -= kfp.K_0 * (angle_m - kfp.angle);
    kfp.PP[0][1] -= kfp.K_0 * (angle_m - kfp.angle);
    kfp.PP[1][0] -= kfp.K_1 * (angle_m - kfp.angle);
    kfp.PP[1][1] -= kfp.K_1 * (angle_m - kfp.angle);

    return kfp;
}

/**********************************************读取参数*********************************************************/

/*读取50次美格接口传来的数据，防止高频噪声*/
void read_initial_acc_gyro(struct FW_GSENSOR_INFO_STRU * g_info, struct FW_GYROSCOPE_INFO_STRU * gy_info, ST_GSENSOR_DRIVER_PARA gsensor_driver_para)
{
    int i;

    float ax_offset = 0;
    float ay_offset = 0;
    float az_offset = 0;
	float at_offset = 0;
    float gyrox_offset = 0;
    float gyroy_offset = 0;
    float gyroz_offset = 0;

    struct FW_GSENSOR_INFO_STRU a_temp = { 0 };
    struct FW_GYROSCOPE_INFO_STRU gy_temp = { 0 };

    /*需要同美格看一下分辨率,100HZ*/
    for (i = 0; i < gsensor_driver_para.reading_count; i++)
    {
        if (SUCCESS != fw_gsensor_get_info(&a_temp))
        {
            //log_e("get gsensor info failed\n");
            continue;
        }

        if (SUCCESS != fw_gyroscope_get_info(&gy_temp))
        {
            //log_e("get gyroscope info failed\n");
            continue;
        }

        ax_offset += a_temp.acc_x;
        ay_offset += a_temp.acc_y;
        az_offset += a_temp.acc_z;
		at_offset += a_temp.temp;
        gyrox_offset += gy_temp.gyro_x;
        gyroy_offset += gy_temp.gyro_y;
        gyroz_offset += gy_temp.gyro_z;

        usleep(10*1000);
    }

    g_info->acc_x = ax_offset / gsensor_driver_para.reading_count;
    g_info->acc_y = ay_offset / gsensor_driver_para.reading_count;
    g_info->acc_z = az_offset / gsensor_driver_para.reading_count;
	g_info->temp  = at_offset / gsensor_driver_para.reading_count;
    gy_info->gyro_x = gyrox_offset / gsensor_driver_para.reading_count;
    gy_info->gyro_y = gyroy_offset / gsensor_driver_para.reading_count;
    gy_info->gyro_z = gyroz_offset / gsensor_driver_para.reading_count;
}

/***********************************************计算参数**************************************************************/

/*计算加速度的变化率 单位：10^(-3)g/cylce*/
void cal_acc_rate(struct FW_GSENSOR_INFO_STRU * g_info, struct FW_GSENSOR_INFO_STRU * kg_info, ST_RATE_ACC * st_rate_acc, ST_ACC * st_acc)
{
    /*因为安装的tbox的方向，加速度xy坐标需要对换*/
    float acc_temp = g_info->acc_x;
    g_info->acc_x = g_info->acc_y;
    g_info->acc_y = acc_temp;

    /*低通滤波器去除高频的加速度噪音,目前看来kg_info已经够用了*/
    kg_info->acc_x =lowPassFilter(g_info->acc_x, kg_info->acc_x );
    kg_info->acc_y =lowPassFilter(g_info->acc_y, kg_info->acc_y );
    kg_info->acc_z =lowPassFilter(g_info->acc_z, kg_info->acc_z );

    /*计算加速度变化率*/
    st_rate_acc->acc_rate = ((fabsf(st_acc->acc_x_last - kg_info->acc_x)) + (fabsf(st_acc->acc_y_last - kg_info->acc_y)) + (fabsf(st_acc->acc_z_last - kg_info->acc_y))) * 1000;
    st_rate_acc->acc_rate_xy = ((fabsf(st_acc->acc_x_last - kg_info->acc_x)) + (fabsf(st_acc->acc_y_last - kg_info->acc_y))) * 1000;
    st_rate_acc->acc_rate_x = (fabsf(st_acc->acc_x_last - kg_info->acc_x)) * 1000;
    st_rate_acc->acc_rate_y = (fabsf(st_acc->acc_y_last - kg_info->acc_x)) * 1000;
    st_acc->acc_x_last = kg_info->acc_x;
    st_acc->acc_y_last = kg_info->acc_y;
    st_acc->acc_z_last = kg_info->acc_z;

    st_acc->acc_xyz = kg_info->acc_x * kg_info->acc_x + kg_info->acc_y * kg_info->acc_y + kg_info->acc_z * kg_info->acc_z;
}

